﻿/*****************************************************************//**
 * \file   HapticDeviceControl.h
 * \brief  simga.7触力反馈设备通讯类
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/

#pragma once

#include "Eigen/Dense"
#include <fstream>

using namespace std;

#define HAPTICTYPE_LEFT 0
#define HAPTICTYPE_RIGHT 1

//simga.7主手实时数据获取类结构体
struct SRobotHapticDeviceData
{
	//机械臂是否连接
	bool bRobotHapticDeviceConnect;

	//位置平移
	double dhd_px, dhd_py, dhd_pz;
	Eigen::Vector3d position;

	//姿态旋转
	double dhd_ox, dhd_oy, dhd_oz;
	Eigen::Matrix3d dhd_orientation;

	//位置与姿态构成的4阶矩阵
	Eigen::Matrix4d dhd_PostureMatrix;

	//simga.7主手夹持角度
	double dChipGrap;

};

class HapticDeviceControl
{
public:
	bool bHapticInitDone;
	int Haptictype, deviceID;
	//simga.7的姿态
	double dhd_matrix[3][3];
	//simga.7的位置与姿态
	Eigen::Matrix4d temp;
	double grippergap;
	Eigen::Vector3d force;

	fstream hapticDeviceRead;

public:
	HapticDeviceControl();
	~HapticDeviceControl();
	void HapticDeviceInitial();
	void OpenAndInitial();
	void HapticDeviceClose();
	void Close();
	void SetGravityCompensation();
	void GravityCompensation();
	void GetHapticPosture(SRobotHapticDeviceData& sHapticRunData);
	Eigen::Matrix4d GetPositionAndOrientation();
	double GetGripperGap();
	void SetForceAndTorqueAndGripperForce(Eigen::Vector3d force);
	void SetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double gripperforce);
	void SetForceAndTorque(double fx, double fy, double fz, double tx, double ty, double tz);
};
